/**
 * @file curvevoxel_cluster.h
 * @author tangjf (ryontang@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-03-11
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#ifndef CLASS_CURVEVOXEL_CLUSTER_H
#define CLASS_CURVEVOXEL_CLUSTER_H

#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <iostream>
#include <unordered_map>
#include <algorithm>
#include <pcl/filters/voxel_grid.h>

namespace lidar
{
namespace obj
{
using namespace std;

struct PointAPR
{
  float azimuth;
  float polar_angle;
  float range;
};

struct Voxel
{
  bool haspoint = false;
  int cluster = -1;
  vector<int> index;
};

struct PointXYZ
{
  float x;
  float y;
  float z;
};

class Curvevoxel_cluster
{

public:
  Curvevoxel_cluster() {}
  ~Curvevoxel_cluster() {}

  template <typename T>
  inline string toString(const T &t)
  {
    ostringstream oss;
    oss << t;
    return oss.str();
  }

  /**
  * @brief 计算y/x的反正切角度，角度按照右手定则，逆时针方向累加
  * 
  * @param x 
  * @param y 
  * @return float 
  */
  float Polar_angle_cal(float x, float y);

  /**
   * @brief 点云数据转换成typedef PointAPR类型，并把所有信息放在vapr，并在这个过程中求APR的最大最小值
   * 
  * @tparam PointT 
  * @param cloud_IN 
  * @param vapr 
   */
  template <typename PointT>
  inline void calculateAPR(const pcl::PointCloud<PointT> &cloud_IN, vector<PointAPR> &vapr)
  {
    for (int i = 0; i < cloud_IN.points.size(); ++i)
    {
      PointAPR par;
      // 水平角
      par.polar_angle = Polar_angle_cal(cloud_IN.points[i].x, cloud_IN.points[i].y);
      par.range = sqrt(cloud_IN.points[i].x * cloud_IN.points[i].x + cloud_IN.points[i].y * cloud_IN.points[i].y);
      // 垂直角
      par.azimuth = (float)atan2(cloud_IN.points[i].z, par.range);
      if (par.azimuth < minazimuth)
      {
        minazimuth = par.azimuth;
      }
      if (par.azimuth > maxazimuth)
      {
        maxazimuth = par.azimuth;
      }
      if (par.range < minrange)
      {
        minrange = par.range;
      }
      if (par.range > maxrange)
      {
        maxrange = par.range;
      }
      vapr.push_back(par);
    }

    length = round((maxrange - minrange) / deltaR);
    // 立体扇形voxel的宽度个数确定
    width = 301;
    height = round((maxazimuth - minazimuth) / deltaA);
  }

  void build_hash_table(const vector<PointAPR> &vapr, unordered_map<int, Voxel> &map_out);

  void find_neighbors(int polar, int range, int azimuth, vector<int> &neighborindex);

  bool most_frequent_value(vector<int> values, vector<int> &cluster_index);

  /**
   * @brief 合并类在CVC中调用
   * 
   * @param cluster_indices 
   * @param idx1 
   * @param idx2 
   */
  void mergeClusters(vector<int> &cluster_indices, int idx1, int idx2);

  /**
  * @brief 聚类
  * 
  * @param map_in 
  * @param vapr 
  * @return vector<int> 
  */
  vector<int> CVC(unordered_map<int, Voxel> &map_in, const vector<PointAPR> &vapr);

  vector<float> hsv2rgb(vector<float> &hsv);

  float minrange = 3;
  float maxrange = 3;
  float minazimuth = 0;
  float maxazimuth = 0;
  float deltaA = 2;
  float deltaR = 0.35;
  float deltaP = 1.2;
  int length = 0;
  int width = 0;
  int height = 0;

private:
};
} // namespace obj
} // namespace lidar

#endif // CLASS_CURVEVOXEL_CLUSTER_H